#include "move_plan_sys.h"
#include <stdio.h>

using namespace std;
ljl::BOOSTPATH *pPATH = new ljl::BOOSTPATH();
geometry_msgs::PoseStamped spose;

double PI = 3.1415926;

void Callback(const geometry_msgs::PoseStampedConstPtr& msg){

    spose = *msg;

    pPATH->mvTrackPath[0] = spose.pose.position.x;
    pPATH->mvTrackPath[1] = spose.pose.position.y;
    pPATH->mvTrackPath[2] = spose.pose.position.z;
    pPATH->mvTrackPath[3] = spose.pose.orientation.w;
    pPATH->mvTrackPath[4] = spose.pose.orientation.x;
    pPATH->mvTrackPath[5] = spose.pose.orientation.y;
    pPATH->mvTrackPath[6] = spose.pose.orientation.z;
  
    if (pPATH->mvTrackPaths.size() == 0){
	pPATH->mvTrackPaths.push_back(pPATH->mvTrackPath);
	return;
    }
    vector<float> v = pPATH->mvTrackPaths.back();
    const float dx = abs(v[0] - spose.pose.position.x);
    const float dy = abs(v[2] - spose.pose.position.z);

    const float temp1 = v[3];
    const float temp2 = spose.pose.orientation.w;
    const float dw = abs(temp1 - temp2);

    if(dx + dy > 0.1 || (dw > 10 && dw < 300)){
	pPATH->mvTrackPaths.push_back(pPATH->mvTrackPath);
    }
    cout << "sub:   "<<pPATH->mvTrackPaths.size() << endl;
}
int main(int argc,char** argv)
{  
    ros::init(argc, argv, "save_track_path");  
    ros::NodeHandle n;

    ros::Subscriber sub = n.subscribe("/vision_pose360",1, Callback);
    ros::spin();

    ljl::TRACKPATH TrackPath;
    string path = "/home/dji/ros_ws/src/move_drive_plan/src/trackPath.txt";
    TrackPath.SavePath(pPATH, path.c_str());
    cout << "save trackpath in " << path << endl;

 return 0;  
}



